/********************************************************
 *
 * FILE NAME:   butterfilter.cpp
 *
 * DESCRIPTION:
 *  Module with Butterworth filter
 *
 ********************************************************/

/********************************************************/
/*              Includes                                */
/********************************************************/
#include "butterfilter.h"

#include <QDebug>
#include <algorithm>

/********************************************************/
/*              Defines                                 */
/********************************************************/

/********************************************************/
/*              Macros                                  */
/********************************************************/

/********************************************************/
/*              Typedefs                                */
/********************************************************/

/********************************************************/
/*              Local Functions Declarations (LOCAL)    */
/********************************************************/

/********************************************************/
/*              Global Variables (GLOBAL/IMPORT)        */
/********************************************************/

/********************************************************/
/*              Local Module Variables (MODULE)         */
/********************************************************/

/********************************************************/
/*              Local File Variables (LOCAL)            */
/********************************************************/

/********************************************************/
/*              Functions Definitions (LOCAL/GLOBAL)    */
/********************************************************/

ButterFilter::ButterFilter(Filter *parent) : Filter(parent){}

void ButterFilter::init(int N,int burstLength)
{
    a = new sample_t[N + 1];
    aLength = N + 1;

    b = new sample_t[N + 1];
    bLength = N + 1;

    order = N;

    input = new ISignal(N + 1, burstLength);
    output = new FilterOSignal(this, N + 1);
}

void ButterFilter::setCoefficients(double Wn,int filterType)
{
    sample_t *tempA = new sample_t[order + 1];
    sample_t *tempB = new sample_t[order + 1];
    sample_t *d = new sample_t[order + 1];

    for(size_t i = 0; i <= order; ++i)
    {
        tempA[i] = 0;
        tempB[i] = 0;
        d[i] = 0;
    }

    sample_t wcd = 2 * M_PI * Wn;
    sample_t wca = 2 * tan(wcd / 2);                // convert digital freq to analog
                                                    //(frequency warping relationship)
    d[0] = 1;
    for (size_t i = 1; i <= order; ++i)
    {                                                                                        // H(p) = 1 / D(p)
        d[i] = (cos((i - 1) * M_PI / (2 * order)) * d[i - 1]) / sin(i * M_PI / (2 * order)); // D(p) = d0 + d1 * p + d2 * p^2 + dn * p^n
    }                                                                                        // p = s/wc lowpass  , p = wc/s  highpass

    switch (filterType)
    {
        case LOWPASS:
            for(size_t i = 0; i <= order; i++)
            {
                tempA[i] = d[i] * (pow(wca, (order - i)));
            }
            tempB[0] = pow(wca, order);
            break;

        case HIGHPASS:
            for (size_t i = 0; i <= order; i++)
            {
                tempA[order - i] = d[i] * (pow(wca, i));
            }
            tempB[order] = 1;
            break;

        default:
            return;
            break;
    }
    if(((Wn == 0) && (filterType == LOWPASS)) \
            ||((Wn == 0.5)&&(filterType == HIGHPASS)))
    {
         for (size_t n = 0; n <= order; n++)
         {
             a[n] = 0;
             b[n] = 0;
         }
         a[0] = 1;
    }
    else if(((Wn == 0.5) && (filterType == LOWPASS)) \
                ||((Wn == 0)&&(filterType == HIGHPASS)))
    {
         for (size_t n = 0; n <= order; n++)
         {
             a[n] = 0;
             b[n] = 0;
         }
         b[0] = 1;
         a[0] = 1;
    }else
    {
        a[0] = bilinearTransform(tempA, order, 0);
        b[0] = bilinearTransform(tempB, order, 0) / a[0];
        for (size_t n = 1; n <= order; n++)
        {
            a[n] = bilinearTransform(tempA, order, n) / a[0];
            b[n] = bilinearTransform(tempB, order, n) / a[0];
        }
        a[0] = 1;
    }

    delete[] tempA;
    delete[] tempB;
    delete[] d;
}

ButterFilter::~ButterFilter()
{
    if(a != nullptr)
    {
        delete[] a;
    }

    if(b != nullptr)
    {
        delete[] b;
    }
}

unsigned int ButterFilter::factorial(unsigned int num)
{
    unsigned int result;

    if(num > 1)
    {
        result = num * factorial(num - 1);
    }else
    {
        result = 1;
    }
    return result;
}

sample_t ButterFilter::bilinearTransform(sample_t *sCoeff, int N, int n)
{
    sample_t zCoeff = 0;
    sample_t acc = 0;

    for(int i = 0; i <= N; ++i)
    {
        acc = 0;
        for(int k = std::max(n - N + i, 0); k <= std::min(i, n); ++k)
        {
            acc += (sample_t(factorial(i) * factorial(N - i)) / (factorial(k) * factorial(i - k) * factorial(n - k) * factorial(N - i - n + k))) * ((k & 1) ? -1 : 1);
        }
        zCoeff += (sCoeff[i] * pow(2, i) * acc);
    }
    return zCoeff;
}
